#pragma once
#include <ros/ros.h>

#include <iostream>
#include <sstream>
#include <ctime>
#include <stdio.h>
#include <fstream>
#include <math.h>
#include <dirent.h>
#include <sys/stat.h>
#include <pwd.h> 

#include <jsoncpp/json/json.h>

#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Geometry>

#include <opencv2/core/core.hpp>
#include "opencv2/core/utility.hpp"
#include "opencv2/core/ocl.hpp"
#include "opencv2/calib3d.hpp"
#include "opencv2/features2d.hpp"
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

using namespace cv;
using namespace std;
using namespace Eigen;
enum RobotWorldHandEyeCalibrationMethod
{
    CALIB_ROBOT_WORLD_HAND_EYE_SHAH = 0, //!< Solving the robot-world/hand-eye calibration problem using the kronecker product @cite Shah2013SolvingTR
    CALIB_ROBOT_WORLD_HAND_EYE_LI = 1    //!< Simultaneous robot-world and hand-eye calibration using dual-quaternions and kronecker product @cite Li2010SimultaneousRA
};

class HEcalibrator
{
private:
    void initRosIO();
    void readFileJson(int count,vector<string> filepath_json);
    void calibrateSensors();
    void saveCalibrationFile(cv::Mat in_extrinsic, cv::Mat in_intrinsic, cv::Mat in_dist_coeff, cv::Size in_size);
    void getFileName(char * dirPath,vector<string>&files,string suffix);

    void calibrateRobotWorldHandEye(InputArrayOfArrays R_world2cam, InputArrayOfArrays t_world2cam,
                                    InputArrayOfArrays R_base2gripper, InputArrayOfArrays t_base2gripper,
                                    OutputArray R_base2world, OutputArray t_base2world,
                                    OutputArray R_gripper2cam, OutputArray t_gripper2cam,
                                    RobotWorldHandEyeCalibrationMethod method);

    void calibrateRobotWorldHandEyeShah(const std::vector<Mat_<double>> &cRw, const std::vector<Mat_<double>> &ctw,
                                        const std::vector<Mat_<double>> &gRb, const std::vector<Mat_<double>> &gtb,
                                        Matx33d &wRb, Matx31d &wtb, Matx33d &cRg, Matx31d &ctg);

    void calibrateRobotWorldHandEyeLi(const std::vector<Mat_<double>> &cRw, const std::vector<Mat_<double>> &ctw,
                                      const std::vector<Mat_<double>> &gRb, const std::vector<Mat_<double>> &gtb,
                                      Matx33d &wRb, Matx31d &wtb, Matx33d &cRg, Matx31d &ctg);

    Mat kron(const Mat &A, const Mat &B);
    Mat_<double> normalizeRotation(const Mat_<double> &R_);

    double orientation_x;
    double orientation_y;
    double orientation_z;
    double orientation_w;
    double position_x;
    double position_y;
    double position_z;
    double square_dimension;

    int chessboard_width;
    int chessboard_height;

    string data_path;

    ros::NodeHandle nh;
    
public:
    void Run();
    HEcalibrator();
    ~HEcalibrator();
};
